// Load the components and engine objects
Engine.include("/components/component.transform2d.js");
Engine.include("/components/component.keyboardinput.js");
Engine.include("/engine/engine.object2d.js");

Engine.initObject("GameObject", "Object2D", function() {
    
    var GameObject = Object2D.extend({
	// Initial states
	initPoint: null,
	initTheta: 0,
	initVel: Point2D.create(0,1),
	initOmega: 0,
	states: null,
	input: $V([0,0]),
	u: null, // Actual thruster inputs
	u1_max: 2, // Maximum thrust allowed for thruster 1
	u2_max: 2, // Maximum thrust allowed for thruster 2
	// Value determined using cvx to generate CAF
	K: $M([[0, -0.1, 0,    0, 0, 0], 
	       [0,    0, 0, -0.1, 0, 0]]),
	color: "#0000ff",		// The color of the object
	obj: null,			// Our object's shapes
	
	constructor: function(startPoint) {
            this.base("GameObject");
	    
            // Add the component to move the object
            this.add(Transform2DComponent.create("move"));
	    
	    // Add the component which handles keyboard input
	    this.add(KeyboardInputComponent.create("input"));
	    
	    // Set our object's shape
	    this.obj = this.create_object();
	    
            // Position the object
	    this.initPoint = startPoint;
            this.setPosition(this.initPoint);
	    
	    this.states = $V([this.initPoint.x,
			      this.initVel.x,
			      this.initPoint.y,
			      this.initVel.y,
			      this.initTheta,
			      this.initOmega]);
	},


	/**
        * Models the dynamics for the system.
        * User defined function that takes three
        * arguments--time, $V state, and $V input--and outputs 
        * a $V of the timer derivative of the state.
        */
	diffeq: function(t, state, input){
	    // Object parameters
	    var a = 1;
	    var b = 1;
	    var inertia = 1;
	    
	    // State values and Inputs
	    var x = state.e(1);
	    var x_dot = state.e(2);
	    var y = state.e(3);
	    var y_dot = state.e(4);
	    var theta = state.e(5);
	    var theta_dot = state.e(6);
	    
	    // Now implement differential equations
	    var x_ddot = -input.e(1)*Math.sin(theta) + 
		input.e(2)*Math.cos(theta) + 1 - x + 0.1*x_dot;
	    var y_ddot =  input.e(1)*Math.cos(theta) + 
		input.e(1)*Math.sin(theta) + 1 - y + 0.1*y_dot;
	    var theta_ddot = input.e(1)*a/inertia - 
		input.e(2)*b/inertia;
	    Engine.addMetric("ydot",y_dot,false,"#");
	    var xdot = $V([x_dot,
		       x_ddot,
		       y_dot,
		       y_ddot,
		       theta_dot,
		       theta_ddot]);
	    return xdot;
	},

	updateStates: function() {
	    // TODO: add in simulation time to give to ode45
	    // Provide trajectory robustness
	    var input = this.input;
	    var K = this.K;
	    var v = input.add(K.multiply(this.states));

	    // Feedback Linearize
	    var theta = this.states.e(5);
	    this.u = $M([[-Math.sin(theta), Math.cos(theta)],
			[Math.cos(theta), Math.sin(theta)]]).multiply(v);

	    this.states = ode45(this.diffeq, this.states, this.u, 0,
				Nonlinear.getTimestep());
	    Engine.addMetric("timestep",Nonlinear.getTimestep(),false,"#s");
	    Engine.addMetric("u1",this.u.e(1),false,"#");
	    Engine.addMetric("u2",this.u.e(2),false,"#");
	},
	
	/**
       * Update the object within the rendering context.  
       * This calls the transform
       * components to position the object on the playfield.
       *
       * @param renderContext {RenderContext} The rendering context
       * @param time {Number} The engine time in milliseconds
       */
	update: function(renderContext, time) {
	    renderContext.pushTransform();
	    
	    // The the "update" method of the super class
	    this.base(renderContext, time);
	    
	    this.updateStates();
	    Engine.addMetric("colorish",rgb2html(0.7,0,1),false,"#");
	    //Engine.addMetric("y",this.states.e(3),false,"#");
	    this.setPosition(Point2D.create(this.states.e(1),
					    this.states.e(3)));
	    this.setRotation(this.states.e(5));

	    // Draw the object on the render context
	    this.draw(renderContext);

	    renderContext.popTransform();

	    // @DEBUG testing out the metrics capabilties
	    var pos = this.getPosition();
	    Engine.addMetric("Xpos", pos.x.toFixed(2), false, "#");
	    Engine.addMetric("Ypos", pos.y.toFixed(2), false, "#");
	},
	
	/**
	 * Handle a "keydown" event from the <tt>KeyboardInputComponent</tt>.
	 * @param keyCode {Number} The key which was pressed down.
	 */
	onKeyDown: function(keyCode) {
	    // switch (keyCode) {
	    // case EventEngine.KEYCODE_LEFT_ARROW:
	    //     this.moveVec.setX(-0.1);
	    //     break;
	    // case EventEngine.KEYCODE_RIGHT_ARROW:
	    //     this.moveVec.setX(0.1);
	    //     break;
	    // case EventEngine.KEYCODE_UP_ARROW:
	    //     this.moveVec.setY(-0.1);
	    //     break;
	    // case EventEngine.KEYCODE_DOWN_ARROW:
	    //     this.moveVec.setY(0.1);
	    //     break;
	    // }
	    return false;
	},
	
	/**
	 * Handle a "keyup" event from the <tt>KeyboardInputComponent</tt>.
	 * @param keyCode {Number} The key which was released
	 */
	onKeyUp: function(keyCode) {
	    // switch (keyCode) {
	    // case EventEngine.KEYCODE_LEFT_ARROW:
	    // case EventEngine.KEYCODE_RIGHT_ARROW:
	    //     this.moveVec.setX(0);
	    //     break;
	    // case EventEngine.KEYCODE_UP_ARROW:
	    // case EventEngine.KEYCODE_DOWN_ARROW:
	    //     this.moveVec.setY(0);
	    //     break;
	    // }
	    return false;
	},
	
	/**
       * Get the position of the object from the transform component.
       * @return {Point2D}
       */
	getPosition: function() {
            return this.getComponent("move").getPosition();
	},

	/**
       * Get the initial position of the object from the transform component.
       * @return {Point2D}
       */
	getInitialPosition: function() {
            return this.initPoint;
	},

	/**
       * Get the rotation of the object from the transform component.
       * @return {radians}
       */
	getRotation: function() {
	    var angle_degrees = this.getComponent("move").getPosition();
	    return angle_degrees*Math.PI/180;
	},
		
	/**
       * Set the position of the object through transform component
       * @param point {Point2D} The position to draw the object in the playfield
       */
	setPosition: function(point) {
            this.base(point);
            this.getComponent("move").setPosition(point);
	},

	/**
       * Set the rotation of the object through transform component
       * @param angle {radians} The angle to draw the object in the playfield
       */
	setRotation: function(angle) {
	    var angle_degrees = angle*180/Math.PI;
            this.base(angle_degrees);
            this.getComponent("move").setRotation(angle_degrees);
	},

	/**
	 * Draw our game object onto the specified render context.
	 * @param renderContext {RenderContext} The context to draw onto
	 */
	draw: function(renderContext) {
	    // Generate a rectangle to represent our object
	    var pos = this.getPosition();
	    
	    // Draw the body
	    renderContext.setFillStyle(this.color);
            renderContext.drawFilledPolygon(this.obj.body);

	    // Set thruster flame color for thruster 1
	    renderContext.setFillStyle(
		rgb2html(1, 1 - Math.abs(this.u.e(1))/this.u1_max, 0));
	    // Draw thruster 1 in the correct direction
	    if(this.u.e(1) > 0){
		renderContext.drawFilledPolygon(this.obj.pos_u1);
	    }
	    else if(this.u.e(1) < 0){
		renderContext.drawFilledPolygon(this.obj.neg_u1);
	    }
	    // Set thruster flame color for thruster 2
	    renderContext.setFillStyle(
		rgb2html(1, 1 - Math.abs(this.u.e(2))/this.u2_max, 0));
	    // Draw thruster 2 in the correct direction
	    if(this.u.e(2) > 0){
		renderContext.drawFilledPolygon(this.obj.pos_u2);		
	    }
	    else if(this.u.e(1) < 0){
		renderContext.drawFilledPolygon(this.obj.neg_u2);
	    }
	},
	/**
         * Create the polygon representing our object
         * Return an array of Point2D's to use with filledPolygon
         */
	create_object: function() {
	    s = .4; // Size scaling factor
	    w = .5*s; // Width
	    h = .3*s; // Height
	    CoM = [0,0]; // Center of Mass
	    a = .5*w/2; // Offset of Thruster 1
	    b = .5*h/2; // Offset of Thruster 2
	    th = .1*s; // Size of Thruster
	    // To simplify the code
	    PI = Math.PI;
	    ang = PI/5;
	    function sin(x){return Math.sin(x)}
	    function cos(x){return Math.cos(x)}
	    function tan(x){return Math.tan(x)} 
	    return {
		width: w,
		height: h,
		body: [Point2D.create(-w/2, h/2),
		       Point2D.create(a - th/4,h/2),
		       Point2D.create(a - th/2,h/2 + th),
		       Point2D.create(a + th/2,h/2 + th),
		       Point2D.create(a + th/4,h/2),
		       Point2D.create(w/2,h/2),
		       Point2D.create(w/2,b + th/4),
		       Point2D.create(w/2 + th,b + th/2),
		       Point2D.create(w/2 + th,b - th/2),
		       Point2D.create(w/2,b - th/4),
		       Point2D.create(w/2,-h/2),
		       Point2D.create(a + th/4,-h/2),
		       Point2D.create(a + th/2,-h/2 - th),
		       Point2D.create(a - th/2,-h/2 - th),
		       Point2D.create(a - th/4,-h/2),
		       Point2D.create(-w/2,-h/2),
		       Point2D.create(-w/2,b - th/4),
		       Point2D.create(-w/2 - th,b - th/2),
		       Point2D.create(-w/2 - th,b + th/2),
		       Point2D.create(-w/2,b + th/4),
		       Point2D.create(-w/2,h/2)],
		
		pos_u1: [Point2D.create(a + 
					th*cos(0*ang - PI/2)/(2*sin(ang)), 
					-h/2 - th - th/(2*tan(ang)) + 
					th*sin(0*ang - PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(2*ang - PI/2)/(2*sin(ang)), 
					-h/2 - th - th/(2*tan(ang)) + 
					th*sin(2*ang - PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(4*ang - PI/2)/(2*sin(ang)), 
					-h/2 - th - th/(2*tan(ang)) + 
					th*sin(4*ang - PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(6*ang - PI/2)/(2*sin(ang)), 
					-h/2 - th - th/(2*tan(ang)) + 
					th*sin(6*ang - PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(8*ang - PI/2)/(2*sin(ang)), 
					-h/2 - th - th/(2*tan(ang)) + 
					th*sin(8*ang - PI/2)/(2*sin(ang)))],

		pos_u2: [Point2D.create(-w/2 - th - th/(2*tan(ang)) + 
					th*cos(0*ang - PI)/(2*sin(ang)), 
					b + 
					th*sin(0*ang - PI)/(2*sin(ang))),
			 Point2D.create(-w/2 - th - th/(2*tan(ang)) + 
					th*cos(2*ang - PI)/(2*sin(ang)), 
					b + 
					th*sin(2*ang - PI)/(2*sin(ang))),
			 Point2D.create(-w/2 - th - th/(2*tan(ang)) + 
					th*cos(4*ang - PI)/(2*sin(ang)), 
					b + 
					th*sin(4*ang - PI)/(2*sin(ang))),
			 Point2D.create(-w/2 - th - th/(2*tan(ang)) + 
					th*cos(6*ang - PI)/(2*sin(ang)), 
					b + 
					th*sin(6*ang - PI)/(2*sin(ang))),
			 Point2D.create(-w/2 - th - th/(2*tan(ang)) + 
					th*cos(8*ang - PI)/(2*sin(ang)), 
					b + 
					th*sin(8*ang - PI)/(2*sin(ang)))],

		neg_u1: [Point2D.create(a + 
					th*cos(0*ang + PI/2)/(2*sin(ang)), 
					h/2 + th + th/(2*tan(ang)) + 
					th*sin(0*ang + PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(2*ang + PI/2)/(2*sin(ang)), 
					h/2 + th + th/(2*tan(ang)) + 
					th*sin(2*ang + PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(4*ang + PI/2)/(2*sin(ang)), 
					h/2 + th + th/(2*tan(ang)) + 
					th*sin(4*ang + PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(6*ang + PI/2)/(2*sin(ang)), 
					h/2 + th + th/(2*tan(ang)) + 
					th*sin(6*ang + PI/2)/(2*sin(ang))),
			 Point2D.create(a + 
					th*cos(8*ang + PI/2)/(2*sin(ang)), 
					h/2 + th + th/(2*tan(ang)) + 
					th*sin(8*ang + PI/2)/(2*sin(ang)))],

		neg_u2: [Point2D.create(w/2 + th + th/(2*tan(ang)) + 
					th*cos(0*ang)/(2*sin(ang)), 
					b + 
					th*sin(0*ang)/(2*sin(ang))),
			 Point2D.create(w/2 + th + th/(2*tan(ang)) + 
					th*cos(2*ang)/(2*sin(ang)), 
					b + 
					th*sin(2*ang)/(2*sin(ang))),
			 Point2D.create(w/2 + th + th/(2*tan(ang)) + 
					th*cos(4*ang)/(2*sin(ang)), 
					b + 
					th*sin(4*ang)/(2*sin(ang))),
			 Point2D.create(w/2 + th + th/(2*tan(ang)) + 
					th*cos(6*ang)/(2*sin(ang)), 
					b + 
					th*sin(6*ang)/(2*sin(ang))),
			 Point2D.create(w/2 + th + th/(2*tan(ang)) + 
					th*cos(8*ang)/(2*sin(ang)), 
					b + 
					th*sin(8*ang)/(2*sin(ang)))]
	    }
	}
	
    }, { // Static
	
	/**
       * Get the class name of this object
       * @return {String} The string MyObject
       */
	getClassName: function() {
            return "GameObject";
	}
    });
    
    return GameObject;
    
});
